#include "IkTestFigure.h"

#define PI 3.14159265
#define BONE_SIZE 0.25

IkTestFigure::IkTestFigure(){
	center = Vector3f(0 , 0.425 , 0);
	
	Link* head = new Link(center, Vector3f(0, 0.625,0));
	Link* spine = new Link(center, Vector3f(0 , -0.065, 0));
	
	Vector3f leftShoulderEnd(-0.13, 0.425, 0);
	Link* leftShoulder = new Link(center, leftShoulderEnd);
	
	Vector3f rightShoulderEnd(0.13, 0.425, 0);
	Link* rightShoulder = new Link(center, rightShoulderEnd);

	Vector3f leftHipEnd(-0.0825, -0.065, 0);
	Link* leftHip = new Link(Vector3f(0 , -0.065, 0), leftHipEnd);
	
	Vector3f rightHipEnd(0.0825, -0.065, 0);
	Link* rightHip = new Link(Vector3f(0 , -0.065, 0), rightHipEnd);

	leftArm.initLink(leftShoulderEnd, Vector3f(0, 0, 1), PI, BONE_SIZE);
	leftArm.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);
	leftArm.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE/3);

	rightArm.initLink(rightShoulderEnd, Vector3f(0, 0, 1), 0, BONE_SIZE);
	rightArm.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);
	rightArm.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE/3);

	
	leftLeg.initLink(leftHipEnd, Vector3f(0, 0, 1), 3*PI/2, BONE_SIZE);
	leftLeg.addLink(Vector3f(0, 0, 1), 0, 1.2*BONE_SIZE);
	leftLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE/3);

	rightLeg.initLink(rightHipEnd, Vector3f(0, 0, 1), 3*PI/2, BONE_SIZE);
	rightLeg.addLink(Vector3f(0, 0, 1), 0, 1.2*BONE_SIZE);
	rightLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE/3);

	body.push_back(head);
	body.push_back(spine);
	body.push_back(leftShoulder);
	body.push_back(rightShoulder);
	body.push_back(leftHip);
	body.push_back(rightHip);
	for(int i=0; i< (int) leftArm.links.size(); i++)
		body.push_back(leftArm.links[i]);

	for(int i=0; i< (int) rightArm.links.size(); i++)
		body.push_back(rightArm.links[i]);

	for(int i=0; i< (int) leftLeg.links.size(); i++)
		body.push_back(leftLeg.links[i]);

	for(int i=0; i< (int) rightLeg.links.size(); i++)
		body.push_back(rightLeg.links[i]);

	control = 0;
}

void IkTestFigure::display(){
	
	for(int i=0; i< (int) body.size(); i++)
		body[i]->draw();
}
void IkTestFigure::move(Vector3f goal){
	switch(control){
	case 0:
		goal -= Vector3f(-0.13, 0.425, 0);
		leftArm.calculate(goal);
		break;
	case 1:
		goal -= Vector3f(0.13, 0.425, 0);
		rightArm.calculate(goal);
		break;
	case 2:
		goal -= Vector3f(-0.0825, -0.065, 0);
		leftLeg.calculate(goal);
		break;
	case 3:
		goal -= Vector3f(0.0825, -0.065, 0);
		rightLeg.calculate(goal);
		break;
	
	}
}

void IkTestFigure::associateLinks(triple* t){
	float min = (t->myVertex - body[0]->getMid()).norm();
	Link* min_Link = body[0];
	for(int i =1; i< (int) body.size(); i++){
		float dist =  (t->myVertex - body[i]->getMid()).norm();
		if(dist< min){
			min = dist;
			min_Link = body[i];

		}
	}
	t->myColor(0) = min_Link->r;
	t->myColor(1) = min_Link->g;
	t->myColor(2) = min_Link->b;

	t->myVertexLocal = min_Link->getLocal(t->myVertex);
	min_Link->vertices.push_back(t);
}

void IkTestFigure::switchBoneControl(){
	control++;
	control = control%4;
}
